;;; ------------ Least Squares method ---------------
;; see Ling Chengsen's book P75

(load "/home/cl/gits/scheme/numerical-analysis/matrix-vector-operations.scm")
(load "/home/cl/gits/libs-for-chezscheme/range.ss")
(import (range))
(load "/home/cl/gits/libs-for-chezscheme/parallelization-in-vector.sls")
(import (parallelization-in-vector))


;;;;;;;; get A^T A = C and A^T b = d
(define (trans-and-product A b n);;only get C_{i,j<=i}
  ;;get d
  (define d
    (vector-map (lambda (i)
                  (apply + (vector->list
                            (vector-map *
                                        (matrix-ref-column A i)
                                        b))))
                (vector-range n)))
  ;;get C
  (define C
    (vector-map/parallel (lambda (i)
                           (vector-map
                            (lambda (j)
                              (apply + (vector->list
                                        (vector-map *
                                                    (matrix-ref-column A i)
                                                    (matrix-ref-column A j)))))
                            (vector-range (+ i 1))))
                         (vector-range n)))
  (cons C d))

;;;;;;;; Cholesky decomposition! -> L
(define (cholesky-decomposition! C n)
  (do ([j 0 (+ j 1)])
      ((= j n) C)
    (matrix-set! C j j
          (sqrt (- (matrix-ref C j j)
                   (do ([k 0 (+ k 1)]
                        [sum 0 (+ sum (expt (matrix-ref C j k) 2))])
                       ((= k j) sum)))))
    (do ([i (+ j 1) (+ i 1)])
        ((= i n))
      (matrix-set! C i j
            (/ (- (matrix-ref C i j)
                  (do ([k 0 (+ k 1)]
                       [sum 0 (+ sum (* (matrix-ref C i k)
                                        (matrix-ref C j k)))])
                      ((= k j) sum)))
               (matrix-ref C j j))))))


;;;;;;;; solve y in L y = d, L is lower-triangular matrix
(define (sol-lower-triangular-eqns! L d n)
  (do ([i 0 (+ i 1)])
      ((= i n) d);;return d (also not)
    (vector-set! d i
                 (/ (- (vector-ref d i)
                       (do ([j 0 (+ j 1)]
                            [sum 0 (+ sum
                                      (* (matrix-ref L i j)
                                         (vector-ref d j)))])
                           ((= j i) sum)))
                    (matrix-ref L i i)))))

;;;;;;;; solve x in L^T x = y, L is lower-triangular matrix
(define (sol-trans-lower-triangular-eqns! L y n);; 
  (do ([i (- n 1) (- i 1)])
      ((= i -1) y)
    (vector-set! y i
                 (/ (- (vector-ref y i)
                       (do ([j (+ i 1) (+ j 1)]
                            [sum 0 (+ sum
                                      (* (matrix-ref L j i)
                                         (vector-ref y j)))])
                           ((= j n) sum)))
                    (matrix-ref L i i)))))

;;;;;;;;;; collect all defined
(define (Least-Squares-method A b)
  (let* ([n (vector-length b)]
         [C-and-d (trans-and-product A b n)]
         [L (cholesky-decomposition! (car C-and-d) n)])
    (sol-trans-lower-triangular-eqns! L
                                      (sol-lower-triangular-eqns! L (cdr C-and-d) n)
                                      n)))




